

#include <TinyGPS.h>
#include <SPI.h>
#include <inttypes.h>
#include <Arduino.h>

#include "globalDefs.h"
#include "LOG.h"
#include "IMU.h"
#include "ExtPROM.h"
//#include "WiFi.h"
#include "WARDEN.h"


#define def_switchDelay 50
//CONTROL CONFIGS
	//#define def_LED_BYTE	//use LEDs for debugging
	//#define def_PRINTMODE  //print contents of EEPROM to PC
	#define def_LOGMODE		//record IMU & GPS data to EEPROM


	#ifdef def_LED_BYTE
		#define BYTE_LED_LSB 6
		int toggles[8]; 
		void toggle(int);
		void byteLite(uint8_t);
	#endif

//Objects
	Warden warden_master;
	Log SpiLOG;
	TinyGPS GPS_handler;
	IMU IMU_handler;
	//WiFi wifi_handler;
	



	int glblSerialMode;
	//SERIAL STATES
		enum {serialMode_self, serialMode_wifi, serialMode_GPS, serialMode_IMU};  //look who's talking

	//GPS control
	enum{def_GPS_start, def_GPS_GA, def_GPS_MC, def_GPS_SA}; 
	uint8_t GPS_state = 0;
	bool flgNewGPS=false;
	int gpsInitIdx;
	int gpsGAIdx;
	unsigned long timeRxGPS=0;
	unsigned long timePrevGPS=0;

	//GPS data
	long lat, lon;
	unsigned long fixage;
	unsigned long GPS_course;
	unsigned long GPS_speed;
	Coordinate mergeCoord;
	int numGPShits=0;

	bool flgNewIMU=false;
	int IMUidx=0;
	//unsigned long timeRxIMU[13]={0,0,0,0,0};

	unsigned long tempTime = 0;
	unsigned long timeLastPrint = 0;
	int lastState=0;

//RX call-back function
	int busStation();

	void serialMode(int);

void setup()
{
	
	//extEEPROM chip select
	pinMode(10, OUTPUT); 
		//and the rest of SPI
	pinMode(11, OUTPUT);
	pinMode(12, INPUT);
	pinMode(13, OUTPUT);

	//rs485 chips
	pinMode(def_selfChip,OUTPUT); //arduino (self)
	pinMode(def_wifiChip,OUTPUT); //wifi
	pinMode(def_GPSChip,OUTPUT); //GPS
	pinMode(def_IMUChip,OUTPUT); //IMU

	SpiPROM.init(10);
	Serial.beginWithInterrupt(9600, busStation);
	//this part will probably have to be moved down to the serial mode function so we can switch baud rates on the fly
			#ifdef def_PRINTMODE
				Serial.begin(def_baudPC);
			#endif

			#ifndef def_PRINTMODE
				serialMode(serialMode_IMU);
			#endif

	#ifdef def_LED_BYTE
	//pinMode(2, OUTPUT);
	//pinMode(3, OUTPUT);
	//pinMode(4, OUTPUT);
	//pinMode(5, OUTPUT);
	pinMode(6, OUTPUT);		
	pinMode(7, OUTPUT);		
	pinMode(8, OUTPUT);		
	pinMode(9, OUTPUT);		
	#endif
	#ifndef def_LED_BYTE

	pinMode(def_Buzzer, OUTPUT);		
	pinMode(def_LED_org, OUTPUT);		
	pinMode(def_LED_red, OUTPUT);
	digitalWrite(def_LED_org, LOW);
	digitalWrite(def_LED_red, LOW);
	digitalWrite(def_Buzzer, LOW);
	#endif

	//tone(def_Buzzer, 920, 800);
	//delay(820);

	delay(5000);

	//digitalWrite(def_LED_org, HIGH);
	serialMode(serialMode_self);
	delay(1000);
	Serial.println("AT+WD");
	Serial.write(13);
		//delay(def_switchDelay);
		//serialMode(serialMode_wifi);
		delay(2750);
		//serialMode(serialMode_self);
		//delay(def_switchDelay);

	Serial.println("AT+WPAPSK=testRouter,00000000");
	Serial.write(13);
		//delay(def_switchDelay);
		//serialMode(serialMode_wifi);
		delay(10500);
		//serialMode(serialMode_self);
		//delay(def_switchDelay);

	Serial.println("AT+WA=testRouter,,6");
	Serial.write(13);
		//delay(def_switchDelay);
		//serialMode(serialMode_wifi);
		delay(3500);
		//serialMode(serialMode_self);
		//delay(def_switchDelay);

	Serial.println("AT+NDHCP=1");    
	Serial.write(13);
		//delay(def_switchDelay);
		//serialMode(serialMode_wifi);
		delay(3500);
		//serialMode(serialMode_self);
		//delay(def_switchDelay);

	Serial.println("AT+NSUDP=80");
	Serial.write(13);
		//delay(def_switchDelay);
		//serialMode(serialMode_wifi);
		delay(4500);
		//serialMode(serialMode_self);
		//delay(def_switchDelay);


	//mergeCoord.lat=0;
	//mergeCoord.lon=0;

	//if(warden_master.inBounds(mergeCoord)){
	//	Serial.println("in");
	//	delay(20);
	//}
	//else
	//{
	//	Serial.println("out");
	//	delay(20);
	//}
	//delay(1000);

	//digitalWrite(def_LED_org, LOW);

	//digitalWrite(def_LED_red, HIGH);
	//delay(800);
	//digitalWrite(def_LED_red, LOW);

	digitalWrite(def_LED_org, HIGH);
	serialMode(serialMode_wifi);
	delay(800);
	digitalWrite(def_LED_org, LOW);

}

//bool bullshitTestToggle=false;




void dataMerge(){
	 mergeCoord.lat = lat;
	 mergeCoord.lon = lon;
 }

void printPage(){
	
	serialMode(serialMode_self);

	SpiLOG.printPage(0);
}

void loop()
{

	//if(glblSerialMode==serialMode_IMU){
	//	
	//	if(numIMU>=5){
	//		serialMode(serialMode_GPS);
	//		numIMU=0;
	//	}
	//}

		
	//if(glblSerialMode==serialMode_GPS){
	//	if(flgNewGPS){
	//		serialMode(serialMode_IMU);
	//		flgNewGPS=false;
	//	}
	//}

	if(flgNewGPS)
	{
			//if(numGPShits>=2){
					delay(15);
					timePrevGPS = timeRxGPS;
					timeRxGPS = millis();

					GPS_course = GPS_handler.course();
					GPS_speed = GPS_handler.speed();

					GPS_handler.get_position(&lat, &lon, &fixage);
					//SpiLOG.addGPS(lat, lon, GPS_handler.hdop());
					
					dataMerge();

					lastState=warden_master.inBounds(mergeCoord);
					if(!lastState)
					{
						digitalWrite(def_LED_red, HIGH);
					}
					else
						digitalWrite(def_LED_red, LOW);

					gpsGAIdx++;


					delay(30);
					serialMode(serialMode_self);
					numGPShits=0;
			//}
			//else
				//timeRxGPS[1] = millis();
					digitalWrite(def_LED_org, LOW);
			flgNewGPS=false;

						//switch(GPS_state){
							//case def_GPS_start:
								//timeRxGPS[gpsInitIdx] = millis();
								//gpsInitIdx++;
								//	if((timeRxGPS[2]-timeRxGPS[0])<300){
								//		GPS_state=def_GPS_GA;
								//	}
								//	if(gpsInitIdx>=3)
								//		gpsInitIdx=0;
							//	break;

							//case def_GPS_GA:
									



									//if(gpsGAIdx>=5){
										//GPS_state=def_GPS_SA;
										//gpsGAIdx=0;
									//}
									//else
									//	GPS_state=def_GPS_MC;
							//	break;

							//case def_GPS_MC:
								//speed and what not
								//GPS_state=def_GPS_GA;
								//serialMode(serialMode_IMU);
							//	break;

							//case def_GPS_SA:
								//sats in sky
								//GPS_state=def_GPS_MC;
								//serialMode(serialMode_self);
						//		break;
						//	default:
						//		break;
						//}
					

		
		
		////if(numGPS<2){  //use this to find GPS interval
		////	if(numGPS==0)
		////		timeTweenGPS = timeRxGPS;
		////	if(numGPS==1)
		////		timeTweenGPS = timeRxGPS - timeTweenGPS;
		////		IMU_handler.init(timeTweenGPS);
		////	//then use 485 chips to switch over to the IMU and take a reading
		////}

		//flgNewGPS=false;
		////serialMode(serialMode_IMU);
		//
		//if(bullshitTestToggle){
		//	digitalWrite(def_LED_red, HIGH);
		//	bullshitTestToggle = !bullshitTestToggle;
		//}
		//else{
		//	digitalWrite(def_LED_red, LOW);
		//	bullshitTestToggle = !bullshitTestToggle;
		//}
	}

	if(flgNewIMU)
	{
		//timeRxIMU[IMUidx] = millis();
		
		//delay(10);

		//SpiLOG.addIMU(IMU_handler.accelX, IMU_handler.accelY, IMU_handler.accelZ, IMU_handler.pitch, IMU_handler.roll, IMU_handler.yaw,IMU_handler.heading);

		//if(timeRxIMU[IMUidx-1]
	}

	//tempTime=millis();
	if(glblSerialMode==serialMode_self) //&& (tempTime-timeRxGPS[gpsGAIdx])<800 && (tempTime-timeRxIMU[IMUidx])<40){
	{
		serialMode(serialMode_self);
		delay(1);
		Serial.println();
		Serial.println();
		Serial.print("@t = ");
		Serial.print(millis());
		Serial.print(" ");
		Serial.print(timeRxGPS-timePrevGPS);
		Serial.print(" ");
		Serial.print(mergeCoord.lat);
		Serial.print(", ");
		Serial.print(mergeCoord.lon);
		Serial.print(" ");
		Serial.println(lastState);

		delay(30);
		serialMode(serialMode_IMU);
	}


}



//uart & rs485 accessories

void serialMode(int mode){  //ensures bus station receives collision free data, could be set to change channel at certain times
	//delay(30);
	//static unsigned long lastBaud=19200;
	switch(mode) {
		case serialMode_self:
			//lastBaud = def_baudPC;
			//Serial.beginWithInterrupt(lastBaud, busStation);
			digitalWrite(def_selfChip, HIGH);
			digitalWrite(def_wifiChip, LOW);
			digitalWrite(def_GPSChip, LOW);
			digitalWrite(def_IMUChip, LOW);
			break;
		case serialMode_wifi:
			//lastBaud = def_baudWIFI;
			//Serial.beginWithInterrupt(lastBaud, busStation);
			digitalWrite(def_selfChip, LOW);
			digitalWrite(def_wifiChip, HIGH); 
			digitalWrite(def_GPSChip, LOW);
			digitalWrite(def_IMUChip, LOW);
			break;
		case serialMode_GPS:
			//digitalWrite(def_LED_red, LOW);
			//lastBaud = def_baudGPS;
			//Serial.beginWithInterrupt(lastBaud, busStation);
			digitalWrite(def_selfChip, LOW);
			digitalWrite(def_wifiChip, LOW);
			digitalWrite(def_GPSChip, HIGH);
			digitalWrite(def_IMUChip, LOW);
			break;
		case serialMode_IMU:
			//digitalWrite(def_LED_red, HIGH);
			//lastBaud = def_baudIMU;
			//Serial.beginWithInterrupt(lastBaud, busStation);
			digitalWrite(def_selfChip, LOW);
			digitalWrite(def_wifiChip, LOW);
			digitalWrite(def_GPSChip, LOW);
			digitalWrite(def_IMUChip, HIGH);
			break;
		default: break;
	}
	glblSerialMode = mode;
}

int busStation(){     //called with each new byte in
	int c = Serial.read();
	static int byteCount=0;
	static unsigned long timeLastPktByte=0;
	if(glblSerialMode==serialMode_self){
		//say something on the bus
		//so this code should never be reached
	}
	else if(glblSerialMode==serialMode_wifi){
		byteCount++;
		if((millis()-timeLastPktByte)>80){
			byteCount=0;
			digitalWrite(def_LED_red, HIGH);
		}

		if(byteCount>888){
			digitalWrite(def_LED_red, LOW);
		}

		timeLastPktByte=millis();

	}
	else if(glblSerialMode==serialMode_GPS){
		if (GPS_handler.encode(c))
		{
			digitalWrite(def_LED_org, HIGH);
			flgNewGPS=true;
			numGPShits++;
		}
		//if(c==36){
		//	flgNewGPS=true;
		//	numGPShits++;
		//}
	}
	else if(glblSerialMode==serialMode_IMU){
		//if (IMU_handler.encode(c))
		//{
		//	flgNewIMU = true;
			IMUidx++;
			if(IMUidx>=400){
				IMUidx=0;
				serialMode(serialMode_GPS);
			}
		//}
	}
	return 1;
}

#ifdef def_LED_BYTE
	void toggle(int bit){
		if(!toggles[bit]){
			digitalWrite((BYTE_LED_LSB+bit), HIGH);
			toggles[bit]=1;
		}

		else{
			digitalWrite((BYTE_LED_LSB+bit), LOW);
			toggles[bit]=0;
		}
	}

	void byteLite(uint8_t byteQ){
		for(int i=0; i<4; i++){
			if(((byteQ>>i) & 0x01)==0x01)
				digitalWrite(BYTE_LED_LSB+i, HIGH);
			else digitalWrite(BYTE_LED_LSB+i, LOW);
		}
	}
#endif